using System;
using System.IO;

namespace Game.Lib
{
    public class Vector3d : Serializable
    {
        #region Constants
        
        public readonly static Vector3d Zero = new Vector3d();
        public readonly static Vector3d XAxis = new Vector3d(1.0, 0.0, 0.0);
        public readonly static Vector3d YAxis = new Vector3d(0.0, 1.0, 0.0);
        public readonly static Vector3d ZAxis = new Vector3d(0.0, 0.0, 1.0);
        public readonly static Vector3d NegXAxis = new Vector3d(-1.0, 0.0, 0.0);
        public readonly static Vector3d NegYAxis = new Vector3d(0.0, -1.0, 0.0);
        public readonly static Vector3d NegZAxis = new Vector3d(0.0, 0.0, -1.0);
        public readonly static Vector3d AllOne = new Vector3d(1.0, 1.0, 1.0);
        public readonly static Vector3d NegAllOne = new Vector3d(-1.0, -1.0, -1.0);
        
        #endregion
        
        #region Fields
        
        public double X;
        public double Y;
        public double Z;
        
        #endregion
        
        #region Constructors
        
        public Vector3d()
        {
            X = Y = Z = 0.0;
        }
        
        public Vector3d(double s)
        {
            X = Y = Z = s;
        }
        
        public Vector3d(double x, double y, double z)
        {
            X = x;
            Y = y;
            Z = z;
        }
        
        public Vector3d(Vector2 a, double z)
        {
            X = a.X;
            Y = a.Y;
            Z = z;
        }
        
        public Vector3d(Vector3d a)
        {
            X = a.X;
            Y = a.Y;
            Z = a.Z;
        }
        
        public Vector3d(Vector3 a)
        {
            X = (double)a.X;
            Y = (double)a.Y;
            Z = (double)a.Z;
        }
        
        #endregion
        
        #region Methods
        
        public void Normalize()
        {
            double mag = Magnitude();
            if (mag > 0.000001)
            {
                mag = 1.0 / mag;
                X *= mag;
                Y *= mag;
                Z *= mag;
            }
            else
            {
                X = Y = Z = 0.0;
            }
        }
        
        public double Magnitude()
        {
            return Math.Sqrt(X * X + Y * Y + Z * Z);
        }
        
        public double MagnitudeSquared()
        {
            return X * X + Y * Y + Z * Z;
        }
        
        public override int GetHashCode()
        {
            return X.GetHashCode() ^ Y.GetHashCode() ^ Z.GetHashCode();
        }
        
        public override string ToString()
        {
            return String.Format(Utils.EnUsCulture, "<{0}, {1}, {2}>", X, Y, Z);
        }

        public override bool Equals(object obj)
        {
            if(!(obj is Vector3d)) return false;
            return this == (Vector3d)obj;
        }
        
        #endregion
        
        #region Static Methods
        
        public static Vector3d Lerp(Vector3d a, Vector3d b, double t)
        {
            return a + (b - a) * t;
        }
        
        public static bool ApproxEqual(Vector3d a, Vector3d b, double err)
        {
            return (a-b).MagnitudeSquared() <= err * err;
        }
        
        public static bool IsFinite(Vector3d a)
        {
            return  Utils.IsFinite(a.X) &&
                    Utils.IsFinite(a.Y) &&
                    Utils.IsFinite(a.Z);
        }
        
        public static Vector3d Clamp(Vector3d test, Vector3d min, Vector3d max)
        {
            return new Vector3d(
			            Utils.Clamp(test.X, min.X, max.X),
			            Utils.Clamp(test.Y, min.Y, max.Y), 
			            Utils.Clamp(test.Z, min.Z, max.Z));
        }
        
        public static double Distance(Vector3d a, Vector3d b)
        {
            return (a - b).Magnitude();
        }
        
        public static double DistanceSquared(Vector3d a, Vector3d b)
        {
            return (a - b).MagnitudeSquared();
        }
        
        public static Vector3d Min(Vector3d a, Vector3d b)
        {
            return new Vector3d(
                Utils.Min(a.X, b.X),
                Utils.Min(a.Y, b.Y),
                Utils.Min(a.Z, b.Z));
        }
        
        public static Vector3d Max(Vector3d a, Vector3d b)
        {
            return new Vector3d(
                Utils.Max(a.X, b.X),
                Utils.Max(a.Y, b.Y),
                Utils.Max(a.Z, b.Z));
        }
        
        public static Vector3d Snap(Vector3d a, double t)
        {
            return new Vector3d(
                Utils.Snap(a.X, t),
                Utils.Snap(a.Y, t),
                Utils.Snap(a.Z, t));
        }
        
        public static Vector3d Parse(string val)
        {
            string[] split = val.Replace("<", String.Empty).Replace(">", String.Empty).Split(new char[] { ',' });
            if(split.Length == 3)
            {
                return new Vector3d(
                    Single.Parse(split[0].Trim(), Utils.EnUsCulture),
                    Single.Parse(split[1].Trim(), Utils.EnUsCulture),
                    Single.Parse(split[2].Trim(), Utils.EnUsCulture));
            }
            else throw new ArgumentException("Vectors must be of the form \"<X, Y, Z>\"");
        }
        
        public static Quaternion RotationBetween(Vector3d a, Vector3d b)
        {
            double dotProduct = a * b;
            double magProduct = a.Magnitude() * b.Magnitude();
            float angle = (float)Math.Acos(dotProduct / magProduct);
            Vector3 axis = new Vector3(a) % new Vector3(b);
            axis.Normalize();
            return new Quaternion(axis, angle);
        }
         
        #endregion
        
        #region Operators
        
        public static Vector3d operator +(Vector3d a, Vector3d b)
        {
            return new Vector3d(a.X + b.X, a.Y + b.Y, a.Z + b.Z);
        }
        
        public static Vector3d operator -(Vector3d a, Vector3d b)
        {
            return new Vector3d(a.X - b.X, a.Y - b.Y, a.Z - b.Z);
        }
        
        public static Vector3d operator -(Vector3d a)
        {
            return new Vector3d(-a.X, -a.Y, -a.Z);
        }
        
        public static Vector3d operator *(double s, Vector3d a)
        {
            return new Vector3d(a.X * s, a.Y * s, a.Z * s);
        }
        
        public static Vector3d operator *(Vector3d a, double s)
        {
            return new Vector3d(a.X * s, a.Y * s, a.Z * s);
        }
        
        public static double operator *(Vector3d a, Vector3d b)
        {
            return a.X * b.X + a.Y * b.Y + a.Z * b.Z;
        }
        
        public static Vector3d operator *(Vector3d vec, Quaternion rot)
        {
            double rw = -rot.X * vec.X - rot.Y * vec.Y - rot.Z * vec.Z;
            double rx = rot.W * vec.X + rot.Y * vec.Z - rot.Z * vec.Y;
            double ry = rot.W * vec.Y + rot.Z * vec.X - rot.X * vec.Z;
            double rz = rot.W * vec.Z + rot.X * vec.Y - rot.Y * vec.X;

            Vector3d a = new Vector3d();

            a.X = -rw * rot.X + rx * rot.W - ry * rot.Z + rz * rot.Y;
            a.Y = -rw * rot.Y + ry * rot.W - rz * rot.X + rx * rot.Z;
            a.Z = -rw * rot.Z + rz * rot.W - rx * rot.Y + ry * rot.X;

            return a;
        }
        
        public static Vector3d operator /(Vector3d vec, Quaternion r)
        {
            Quaternion rot = ~r;
            double rw = -rot.X * vec.X - rot.Y * vec.Y - rot.Z * vec.Z;
            double rx = rot.W * vec.X + rot.Y * vec.Z - rot.Z * vec.Y;
            double ry = rot.W * vec.Y + rot.Z * vec.X - rot.X * vec.Z;
            double rz = rot.W * vec.Z + rot.X * vec.Y - rot.Y * vec.X;

            Vector3d a = new Vector3d();

            a.X = -rw * rot.X + rx * rot.W - ry * rot.Z + rz * rot.Y;
            a.Y = -rw * rot.Y + ry * rot.W - rz * rot.X + rx * rot.Z;
            a.Z = -rw * rot.Z + rz * rot.W - rx * rot.Y + ry * rot.X;

            return a;
        }
        
        
        public static Vector3d operator /(Vector3d a, double s)
        {
            return a * (1.0 / s);
        }
        
        public static Vector3d operator %(Vector3d a, Vector3d b)
        {
            return new Vector3d(
                a.Y * b.Z - b.Y * a.Z,
                a.Z * b.X - b.Z * a.X,
                a.X * b.Y - b.X * a.Y);
        }
        
        public static bool operator ==(Vector3d a, Vector3d b)
        {
            return  a.X == b.X &&
                    a.Y == b.Y &&
                    a.Z == b.Z;
        }
        
        public static bool operator !=(Vector3d a, Vector3d b)
        {
            return  a.X != b.X ||
                    a.Y != b.Y ||
                    a.Z != b.Z;
        }
        
        #endregion
        
        #region Serialization

        public void FromBytes(byte[] data, int pos)
        {
            if(!BitConverter.IsLittleEndian)
            {
                Array.Reverse(data, pos, 8);
                Array.Reverse(data, pos + 8, 8);
                Array.Reverse(data, pos + 16, 8);
            }
            X = BitConverter.ToDouble(data, pos);
            Y = BitConverter.ToDouble(data, pos + 8);
            Z = BitConverter.ToDouble(data, pos + 16);
        }

        public byte[] ToBytes()
        {
            byte[] dest = new byte[24];
            Buffer.BlockCopy(BitConverter.GetBytes(X), 0, dest, 0, 8);
            Buffer.BlockCopy(BitConverter.GetBytes(Y), 0, dest, 8, 8);
            Buffer.BlockCopy(BitConverter.GetBytes(Z), 0, dest, 16, 8);
            if(!BitConverter.IsLittleEndian)
            {
                Array.Reverse(dest, 0, 8);
                Array.Reverse(dest, 8, 8);
                Array.Reverse(dest, 16, 8);
            }
            return dest;
        }
        
        public void Serialize(Stream s)
        {
            if(!s.CanWrite) throw new ArgumentException("Serialization requires a writable stream");
            s.Write(ToBytes(), 0, 24);
        }
        
        public void Deserialize(Stream s)
        {
            if(!s.CanRead) throw new ArgumentException("Serialization requires a writable stream");
            byte[] buf = new byte[24];
            if(s.Read(buf, 0, 24) != 24) throw new EndOfStreamException("Quaternions require at least 16 bytes of data");
            if(!BitConverter.IsLittleEndian)
            {
                Array.Reverse(buf, 0, 8);
                Array.Reverse(buf, 8, 8);
                Array.Reverse(buf, 16, 8);
            }
            X = BitConverter.ToDouble(buf, 0);
            Y = BitConverter.ToDouble(buf, 8);
            Z = BitConverter.ToDouble(buf, 16);
        }
        
        #endregion
    }
}
